/**
 * multiconnect sample
 */
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/sys/byteorder.h>

#include "central_multiconn_client.h"

LOG_MODULE_REGISTER(app, LOG_LEVEL_DBG);

#define SCAN_INTERVAL 0x0030 /* 1000 ms */
#define SCAN_WINDOW 0x0030   /* 30 ms */
#define INIT_INTERVAL 0x0010 /* 10 ms */
#define INIT_WINDOW 0x0010   /* 10 ms */
#define CONN_INTERVAL 0x0064 /* 100 ms */
#define CONN_LATENCY 0
#define CONN_TIMEOUT MIN(MAX((CONN_INTERVAL * 125 *               \
                              MAX(CONFIG_BT_MAX_CONN, 6) / 1000), \
                             10),                                 \
                         3200)

//---- filter 1
//  0000ff01-3c17-d293-8e48-14fe2e4da212
#define UUID_VENDOR_SERVICE_VAL BT_UUID_128_ENCODE(0x0000ff01, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212)
#define UUID_VENDOR_CHAR_WRITE_VAL BT_UUID_128_ENCODE(0x0000ff02, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212)
#define UUID_VENDOR_CHAR_NOTIFY_VAL BT_UUID_128_ENCODE(0x0000ff03, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212)
// #define UUID_VENDOR_CHAR_READ_VAL BT_UUID_128_ENCODE(0xVENDOR, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212)
// #define UUID_VENDOR_CHAR_INDICATE_VAL BT_UUID_128_ENCODE(0xVENDOR, 0x3c17, 0xd293, 0x8e48, 0x14fe2e4da212)

static struct bt_uuid_128 uuid_vendor_service = BT_UUID_INIT_128(UUID_VENDOR_SERVICE_VAL);
static struct bt_uuid_128 uuid_vendor_char_write = BT_UUID_INIT_128(UUID_VENDOR_CHAR_WRITE_VAL);
static struct bt_uuid_128 uuid_vendor_char_notify = BT_UUID_INIT_128(UUID_VENDOR_CHAR_NOTIFY_VAL);
// static struct bt_uuid_128 uuid_vendor_char_read = BT_UUID_INIT_128(UUID_VENDOR_CHAR_READ_VAL);
// static struct bt_uuid_128 uuid_vendor_char_indicate = BT_UUID_INIT_128(UUID_VENDOR_CHAR_INDICATE_VAL);

//---- filter 2
#define UUID_VENDOR_SERVICE_VAL_2 BT_UUID_128_ENCODE(0x0000ff01, 0x4c72, 0x4dd5, 0xb86f, 0x412340a15dad)
#define UUID_VENDOR_CHAR_WRITE_VAL_2 BT_UUID_128_ENCODE(0x0000ff02, 0x4c72, 0x4dd5, 0xb86f, 0x412340a15dad)
#define UUID_VENDOR_CHAR_NOTIFY_VAL_2 BT_UUID_128_ENCODE(0x0000ff03, 0x4c72, 0x4dd5, 0xb86f, 0x412340a15dad)

static struct bt_uuid_128 uuid_vendor_service_2 = BT_UUID_INIT_128(UUID_VENDOR_SERVICE_VAL_2);
static struct bt_uuid_128 uuid_vendor_char_write_2 = BT_UUID_INIT_128(UUID_VENDOR_CHAR_WRITE_VAL_2);
static struct bt_uuid_128 uuid_vendor_char_notify_2 = BT_UUID_INIT_128(UUID_VENDOR_CHAR_NOTIFY_VAL_2);

typedef enum
{
    APP_CONN_STATE_IDLE,
    APP_CONN_STATE_CONNECTING,
    APP_CONN_STATE_READY,
    APP_CONN_STATE_DISCONNECTED
} app_conn_state_t;

typedef struct
{
    bt_addr_le_t addr;
    struct bt_conn *conn_ref; // Store conn ptr if needed by app directly
    app_conn_state_t app_state;
    // Store handles needed by the application
    uint16_t write_handle;
    uint16_t notify_handle;
    uint16_t notify_ccc_handle;
    uint16_t read_handle;
    uint16_t indicate_handle;
    uint16_t indicate_ccc_handle;
    // Add other app-specific data for this connection
    uint32_t message_counter;
} app_conn_data_t;

static app_conn_data_t app_connections[CONFIG_BT_MAX_CONN];

static bool scan_active = false;

static char *filter_addr_str[] = {
    "EF:28:4B:24:BF:4A", // v1220
    "CF:C3:75:79:67:99",
    "F2:84:8E:81:38:0A",
    "F9:17:60:B6:7B:36",
    "E3:6F:95:8E:C6:91",
    // "FD:18:F0:10:99:65", // V3100
    "E1:8C:5A:AA:3B:9A", // V3100 52840 dongle
    // "C9:6F:E1:EB:FE:37", // v2100
};

static int start_scan(void);

static app_conn_data_t *find_app_data(struct bt_conn *conn, const bt_addr_le_t *addr)
{
    for (int i = 0; i < CONFIG_BT_MAX_CONN; i++)
    {
        if (conn && app_connections[i].conn_ref == conn)
        {
            return &app_connections[i];
        }
        if (addr && app_connections[i].app_state != APP_CONN_STATE_IDLE &&
            bt_addr_le_cmp(addr, &app_connections[i].addr) == 0)
        {
            return &app_connections[i];
        }
    }
    return NULL;
}

static app_conn_data_t *find_free_app_data_slot(void)
{
    for (int i = 0; i < CONFIG_BT_MAX_CONN; i++)
    {
        if (app_connections[i].app_state == APP_CONN_STATE_IDLE)
        {
            return &app_connections[i];
        }
    }
    return NULL;
}

static int find_connected_app_data_slot(void)
{
    int sum = 0;
    for (int i = 0; i < CONFIG_BT_MAX_CONN; i++)
    {
        if (app_connections[i].app_state == APP_CONN_STATE_READY)
        {
            ++sum;
        }
    }
    return sum;
}

static uint8_t notify_indicate_cb(struct bt_conn *conn,
                                  struct bt_gatt_subscribe_params *params,
                                  const void *data, uint16_t length)
{
    if (!data)
    {
        LOG_INF("[Conn %p] Unsubscribed from handle 0x%04x", (void *)conn, params->value_handle);
        params->value_handle = 0U; // Mark as unsubscribed
        return BT_GATT_ITER_STOP;
    }

    app_conn_data_t *app_data = find_app_data(conn, NULL);
    if (!app_data)
    {
        LOG_ERR("Notify/Indicate CB: App data not found for conn %p", (void *)conn);
        return BT_GATT_ITER_CONTINUE; // Or STOP?
    }

    // Identify which characteristic triggered the callback based on params->value_handle
    const char *char_name = "Unknown";
    if (params->value_handle == app_data->notify_handle)
    {
        char_name = "Notify Char";
    }
    else if (params->value_handle == app_data->indicate_handle)
    {
        char_name = "Indicate Char";
        // Note: Indications require confirmation (handled by stack)
    }

    LOG_INF("[Conn %p] Received %s: Handle 0x%04x, Length %u", (void *)conn, char_name, params->value_handle, length);
    // LOG_HEXDUMP_DBG(data, length, "Data:"); // Optional: Dump data

    // Process received data...
    app_data->message_counter++;

    return BT_GATT_ITER_CONTINUE; // Continue receiving
}

static void on_ready(struct multiconn_conn_context *ctx, int err)
{
    app_conn_data_t *app_data = find_app_data(ctx->conn, NULL);
    if (!app_data)
    {
        LOG_ERR("READY CB: App data not found for conn %p", (void *)ctx->conn);
        return;
    }

    if (err)
    {
        LOG_ERR("[Conn %p] Library initialization failed (err %d)", (void *)ctx->conn, err);
        app_data->app_state = APP_CONN_STATE_DISCONNECTED; // Mark as unusable
        // Consider disconnecting? bt_conn_disconnect(ctx->conn, ...);
        bt_conn_disconnect(ctx->conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
        return;
    }

    LOG_INF("[Conn %p] Library initialization successful. Device Ready.", (void *)ctx->conn);
    app_data->app_state = APP_CONN_STATE_READY;
    app_data->message_counter = 0;

    sys_snode_t *svc_node;
    multiconn_service_t *vendor_service = NULL;

    // Find the vendor service
    SYS_SLIST_FOR_EACH_NODE(&ctx->services, svc_node)
    {
        multiconn_service_t *svc = CONTAINER_OF(svc_node, multiconn_service_t, node);
        if (bt_uuid_cmp((const struct bt_uuid *)&svc->uuid, &uuid_vendor_service.uuid) == 0)
        {
            vendor_service = svc;
            char uuid_str[BT_UUID_STR_LEN];
            bt_uuid_to_str((const struct bt_uuid *)&svc->uuid, uuid_str, sizeof(uuid_str));
            LOG_INF("  Found Vendor Service: %s (Handles 0x%04x-0x%04x)", uuid_str, svc->start_handle, svc->end_handle);
            break;
        }
    }

    if (!vendor_service)
    {
        LOG_ERR("  Vendor service not found!");
        // Handle error - maybe disconnect
        return;
    }

    // Find characteristics within the vendor service
    multiconn_characteristic_t *ch;

    // Write Characteristic
    ch = multiconn_client_find_characteristic_with_props(vendor_service, &uuid_vendor_char_write.uuid,
                                                         BT_GATT_CHRC_WRITE | BT_GATT_CHRC_WRITE_WITHOUT_RESP);
    if (ch)
    {
        app_data->write_handle = ch->value_handle;
        LOG_INF("    Found Write Char: value=0x%04x, props=0x%02x", ch->value_handle, ch->properties);
    }
    else
    {
        LOG_WRN("    Write Char not found.");
    }

    // Notify Characteristic
    ch = multiconn_client_find_characteristic_with_props(vendor_service, &uuid_vendor_char_notify.uuid,
                                                         BT_GATT_CHRC_NOTIFY);
    if (ch)
    {
        app_data->notify_handle = ch->value_handle;
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
        app_data->notify_ccc_handle = ch->ccc_handle;
        LOG_INF("    Found Notify Char: value=0x%04x, ccc=0x%04x", ch->value_handle, ch->ccc_handle);
        // Prepare subscription parameters (using the char's sub_params instance)
        // We need to set the notify callback here.
        ch->sub_params.notify = notify_indicate_cb;
        ch->sub_params.value = BT_GATT_CCC_NOTIFY;      // Value to enable notifications
        ch->sub_params.value_handle = ch->value_handle; // Ensure value handle is set
        ch->sub_params.ccc_handle = ch->ccc_handle;     // Ensure CCC handle is set
                                                        // Atomic flags might be needed if subscribing from multiple threads
                                                        // atomic_set_bit(ch->sub_params.flags, BT_GATT_SUBSCRIBE_FLAG_VOLATILE);
#else
        LOG_INF("    Found Notify Char: value=0x%04x (Desc Discovery Disabled)", ch->value_handle);
#endif
    }
    else
    {
        LOG_WRN("    Notify Char not found.");
    }

#if !defined(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC) // Only manual subscribe if auto is off
    if (app_data->notify_handle != 0 && app_data->notify_ccc_handle != 0)
    {
        multiconn_characteristic_t *notify_chara_for_sub = multiconn_client_find_characteristic(vendor_service, &uuid_vendor_char_notify.uuid);
        if (notify_chara_for_sub)
        {
            LOG_INF("  Attempting manual subscribe to Notify Char (handle 0x%04x)", app_data->notify_ccc_handle);
            err = bt_gatt_subscribe(ctx->conn, &notify_chara_for_sub->sub_params);
            if (err && err != -EALREADY)
            {
                LOG_ERR("  Manual subscribe failed (err %d)", err);
            }
            else
            {
                LOG_INF("  Manual subscribe request sent or already subscribed.");
            }
        }
    }
#endif

    // Send alarm data
    if (app_data->write_handle != 0)
    {
        static uint8_t initial_data[] = {0x8F, 0xD7, 0x01, 0x00, 0x01, 0x01};
        LOG_INF("  Attempting initial write to handle 0x%04x", app_data->write_handle);
        // Use write_without_response if supported and desired
        err = bt_gatt_write_without_response(ctx->conn, app_data->write_handle, initial_data, sizeof(initial_data), false);
        if (err)
        {
            LOG_ERR("  Initial write failed (err %d)", err);
        }
        else
        {
            LOG_INF("  Initial write sent.");
        }
    }

    if (!scan_active)
        start_scan();
}

static void on_error(const bt_addr_le_t *addr, uint8_t err)
{
    // clear
    app_conn_data_t *app_data = find_app_data(NULL, addr);
    if (app_data)
    {
        char addr_str[BT_ADDR_LE_STR_LEN];
        bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
        LOG_ERR("Reset device %s", addr_str);

        app_data->app_state = APP_CONN_STATE_IDLE;
        if (app_data->conn_ref)
        {
            bt_conn_unref(app_data->conn_ref); // Release app's reference if held
            app_data->conn_ref = NULL;
        }

        memset(app_data, 0, sizeof(app_conn_data_t));
    
        // Mark the slot completely free
        app_data->app_state = APP_CONN_STATE_IDLE;
        memset(&app_data->addr, 0, sizeof(app_data->addr));
    }

    if (!scan_active)
        start_scan();
}

static void on_disconnected(struct multiconn_conn_context *ctx, uint8_t reason)
{
    app_conn_data_t *app_data = find_app_data(ctx->conn, NULL);
    if (!app_data)
    {
        // This might happen if disconnect occurs before context is fully associated in app_data
        LOG_WRN("DISCONNECTED CB: App data not found for conn %p", (void *)ctx->conn);
        // Try finding by address if context still holds it? Unreliable here.
        if (!scan_active)
            start_scan();
        return;
    }

    LOG_WRN("[Conn %p] Application layer notified of disconnection (reason 0x%02x)", (void *)ctx->conn, reason);

    // Clean up application state for this connection
    app_data->app_state = APP_CONN_STATE_DISCONNECTED;
    if (app_data->conn_ref)
    {
        bt_conn_unref(app_data->conn_ref); // Release app's reference if held
        app_data->conn_ref = NULL;
    }
    // Optionally clear handles? Depends if app wants to retry connection with same addr
    // memset(app_data, 0, sizeof(app_conn_data_t)); // Or just reset state and handles
    app_data->write_handle = 0;
    app_data->notify_handle = 0;
    app_data->notify_ccc_handle = 0;
    app_data->read_handle = 0;
    app_data->indicate_handle = 0;
    app_data->indicate_ccc_handle = 0;

    // Mark the slot completely free
    app_data->app_state = APP_CONN_STATE_IDLE;
    memset(&app_data->addr, 0, sizeof(app_data->addr));

    if (!scan_active)
        start_scan();
}

static void scan_cb(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
                    struct net_buf_simple *ad)
{
    char addr_str[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
    bool found = false;
    for (int i = 0; i < ARRAY_SIZE(filter_addr_str); i++)
    {
        if (memcmp(addr_str, filter_addr_str[i], strlen(filter_addr_str[i])) == 0)
        {
            found = true;
            break;
        }
    }

    if (!found)
    {
        // printk("Device not in filter list: %s (RSSI %d)\n", addr_str, rssi);
        return;
    }

    printk("Device found: %s (RSSI %d)\n", addr_str, rssi);

    // 停止扫描
    if (scan_active)
    {
        int err = bt_le_scan_stop();
        if (err)
        {
            LOG_ERR("Failed to stop scan (err %d)", err);
        }
        else
        {
            scan_active = false;
        }
    }

    // Check if already connected or connecting to this address in app layer
    app_conn_data_t *app_data = find_app_data(NULL, addr);
    if (app_data && (app_data->app_state == APP_CONN_STATE_CONNECTING || app_data->app_state == APP_CONN_STATE_READY))
    {
        LOG_WRN("Already connected/connecting to %s. Skipping.", addr_str);
        // 重新打开扫描
        if (!scan_active)
            start_scan();
        return;
    }

    // Find a free slot in application data
    app_conn_data_t *free_slot = find_free_app_data_slot();
    if (!free_slot)
    {
        LOG_WRN("No free app connection slots available.");
        // 重新打开扫描
        if (!scan_active)
            start_scan();
        return;
    }

    // 尝试连接
    struct bt_conn_le_create_param create_param = {
        .options = BT_CONN_LE_OPT_NONE,
        .interval = INIT_INTERVAL,
        .window = INIT_WINDOW,
        .interval_coded = 0,
        .window_coded = 0,
        .timeout = 0,
    };
    struct bt_le_conn_param conn_param = {
        .interval_min = CONN_INTERVAL,
        .interval_max = CONN_INTERVAL,
        .latency = CONN_LATENCY,
        .timeout = CONN_TIMEOUT,
    };

    struct bt_conn *conn = NULL;
    int err = bt_conn_le_create(addr, &create_param, &conn_param, &conn);
    if (err)
    {
        LOG_ERR("Failed to create connection to %s (err %d)", addr_str, err);
        // 重新打开扫描
        if (!scan_active)
            start_scan();
        return;
    }
    else
    {
        LOG_INF("Connection pending to %s...", addr_str);
        // Mark slot as connecting in app layer
        free_slot->app_state = APP_CONN_STATE_CONNECTING;
        bt_addr_le_copy(&free_slot->addr, addr);
        free_slot->conn_ref = bt_conn_ref(conn); // Hold app reference during connection attempt
        bt_conn_unref(conn);                     // Decrement the reference count from create call, lib will ref on connected CB
                                                 // Library will handle the connected event via mcc_connected
        conn = NULL;
    }
}

static int start_scan(void)
{
    int err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, scan_cb);
    if (err)
    {
        LOG_ERR("Scanning failed to start (err %d)", err);
    }
    else
    {
        LOG_INF("Scanning started.");
        scan_active = true;
    }
    return err;
}

int main(void)
{
    int err;
    LOG_INF("Starting Multi-Connection Central Example...");

    // 初始化连接数据信息
    memset(app_connections, 0, sizeof(app_connections));
    for (int i = 0; i < CONFIG_BT_MAX_CONN; ++i)
    {
        app_connections[i].app_state = APP_CONN_STATE_IDLE;
    }

    // 初始化协议栈
    err = bt_enable(NULL);
    if (err)
    {
        LOG_ERR("Bluetooth init failed (err %d)", err);
        return err;
    }
    LOG_INF("Bluetooth initialized.");

    // 初始化多连接库
    static multiconn_client_callbacks_t client_cbs = {
        .ready = on_ready,
        .disconnected = on_disconnected,
        .error = on_error,
    };

    err = multiconn_client_init(&client_cbs);
    if (err)
    {
        LOG_ERR("Multi-connection client library init failed (err %d)", err);
        return 0;
    }

    // Set filter to only discover vendor service and specific chars
    const struct bt_uuid *filter_svcs[] = {
        &uuid_vendor_service.uuid,
        &uuid_vendor_service_2.uuid,
    };

    const struct bt_uuid *filter_chars[] = {
        &uuid_vendor_char_write.uuid,
        &uuid_vendor_char_notify.uuid,
        // &uuid_vendor_char_read.uuid,
        // &uuid_vendor_char_indicate.uuid,
        &uuid_vendor_char_write_2.uuid,
        &uuid_vendor_char_notify_2.uuid,
    };
    multiconn_client_set_filter(filter_svcs, ARRAY_SIZE(filter_svcs),
                                filter_chars, ARRAY_SIZE(filter_chars));

    // Set list of characteristics to auto-enable CCCDs for
#if defined(CONFIG_MULTICONN_CLIENT_AUTO_ENABLE_CCC)
    const struct bt_uuid *const auto_enable_list[] = {
        &uuid_vendor_char_notify.uuid, // Automatically enable notify
        // &uuid_vendor_char_indicate.uuid, // Optionally auto-enable indicate too
    };
    multiconn_client_set_auto_enable_list(auto_enable_list, ARRAY_SIZE(auto_enable_list));
#endif

    // 开始扫描
    err = start_scan();
    if (err)
    {
        LOG_ERR("Failed to start scanning (err %d)", err);
        return err;
    }
    LOG_INF("Scanning started.");

    while (1)
    {
        printk("\r\n[Connected devices] -----> %d\r\n", find_connected_app_data_slot());
        k_sleep(K_SECONDS(1));
    }

    return 0;
}
